#include <servo_extend.h>

#include "cmsis_os.h"
#include "stdarg.h"

#define FRAME_HEADER 0x55             // 帧头
#define CMD_SERVO_MOVE 0x03           // 舵机移动指令
#define CMD_ACTION_GROUP_RUN 0x06     // 运行动作组指令
#define CMD_ACTION_GROUP_STOP 0x07    // 停止动作组指令
#define CMD_ACTION_GROUP_SPEED 0x0B   // 设置动作组运行速度
#define CMD_GET_BATTERY_VOLTAGE 0x0F  // 获取电池电压指令

// 宏函数 获得A的低八位
#define GET_LOW_BYTE(A) ((uint8_t)(A))
// 宏函数 获得A的高八位
#define GET_HIGH_BYTE(A) ((uint8_t)((A) >> 8))

UART_HandleTypeDef *SE_huart;  // 舵机控制器句柄

uint8_t LobotTxBuf[128];  // 发送缓存
uint8_t LobotRxBuf[16];
uint16_t batteryVolt;

void SE_init(UART_HandleTypeDef *huart) {
    SE_huart = huart;                                // 初始化舵机控制器
    HAL_UART_Receive_DMA(SE_huart, LobotRxBuf, 16);  // 开启DMA接收
}

void SE_move_servo(uint8_t servoID, uint16_t Position, uint16_t Time) {
    if (servoID > 31 || !(Time > 0)) {  // 舵机ID不能打于31,可根据对应控制板修改
        return;
    }
    LobotTxBuf[0] = LobotTxBuf[1] = FRAME_HEADER;  // 填充帧头
    LobotTxBuf[2] = 8;
    LobotTxBuf[3] =
        CMD_SERVO_MOVE;  // 数据长度=要控制舵机数*3+5，此处=1*3+5//填充舵机移动指令
    LobotTxBuf[4] = 1;                        // 要控制的舵机个数
    LobotTxBuf[5] = GET_LOW_BYTE(Time);       // 取得时间的低八位
    LobotTxBuf[6] = GET_HIGH_BYTE(Time);      // 取得时间的高八位
    LobotTxBuf[7] = servoID;                  // 舵机ID
    LobotTxBuf[8] = GET_LOW_BYTE(Position);   // 取得目标位置的低八位
    LobotTxBuf[9] = GET_HIGH_BYTE(Position);  // 取得目标位置的高八位

    HAL_UART_Transmit(SE_huart, LobotTxBuf, 10, HAL_MAX_DELAY);  // 发送数据
}

void SE_move_servos_by_array(LobotServo servos[], uint8_t Num, uint16_t Time) {
    uint8_t index = 7;
    uint8_t i = 0;

    if (Num < 1 || Num > 32 || !(Time > 0)) {
        return;  // 舵机数不能为零和大与32，时间不能为零
    }
    LobotTxBuf[0] = LobotTxBuf[1] = FRAME_HEADER;  // 填充帧头
    LobotTxBuf[2] = Num * 3 + 5;          // 数据长度 = 要控制舵机数*3+5
    LobotTxBuf[3] = CMD_SERVO_MOVE;       // 填充舵机移动指令
    LobotTxBuf[4] = Num;                  // 要控制的舵机个数
    LobotTxBuf[5] = GET_LOW_BYTE(Time);   // 取得时间的低八位
    LobotTxBuf[6] = GET_HIGH_BYTE(Time);  // 取得时间的高八位

    for (i = 0; i < Num; i++) {              // 循环填充舵机ID和对应目标位置
        LobotTxBuf[index++] = servos[i].ID;  // 填充舵机ID
        LobotTxBuf[index++] =
            GET_LOW_BYTE(servos[i].Position);  // 填充目标位置低八位
        LobotTxBuf[index++] =
            GET_HIGH_BYTE(servos[i].Position);  // 填充目标位置高八位
    }

    HAL_UART_Transmit(SE_huart, LobotTxBuf, LobotTxBuf[2] + 2,
                      HAL_MAX_DELAY);  // 发送数据
}

void SE_move_servos(uint8_t Num, uint16_t Time, ...) {
    uint8_t index = 7;
    uint8_t i = 0;
    uint16_t temp;
    va_list arg_ptr;  //

    va_start(arg_ptr, Time);  // 取得可变参数首地址
    if (Num < 1 || Num > 32) {
        return;  // 舵机数不能为零和大与32，时间不能小于0
    }
    LobotTxBuf[0] = LobotTxBuf[1] = FRAME_HEADER;  // 填充帧头
    LobotTxBuf[2] = Num * 3 + 5;          // 数据长度 = 要控制舵机数 * 3 + 5
    LobotTxBuf[3] = CMD_SERVO_MOVE;       // 舵机移动指令
    LobotTxBuf[4] = Num;                  // 要控制舵机数
    LobotTxBuf[5] = GET_LOW_BYTE(Time);   // 取得时间的低八位
    LobotTxBuf[6] = GET_HIGH_BYTE(Time);  // 取得时间的高八位

    for (i = 0; i < Num;
         i++) {  // 从可变参数中取得并循环填充舵机ID和对应目标位置
        temp = va_arg(arg_ptr, int);  // 可参数中取得舵机ID
        LobotTxBuf[index++] = GET_LOW_BYTE(((uint16_t)temp));
        temp = va_arg(arg_ptr, int);  // 可变参数中取得对应目标位置
        LobotTxBuf[index++] =
            GET_LOW_BYTE(((uint16_t)temp));         // 填充目标位置低八位
        LobotTxBuf[index++] = GET_HIGH_BYTE(temp);  // 填充目标位置高八位
    }

    va_end(arg_ptr);  // 置空arg_ptr

    HAL_UART_Transmit(SE_huart, LobotTxBuf, LobotTxBuf[2] + 2,
                      HAL_MAX_DELAY);  // 发送数据
}

void SE_get_battery_voltage(void) {
    //	uint16_t Voltage = 0;
    LobotTxBuf[0] = FRAME_HEADER;  // 填充帧头
    LobotTxBuf[1] = FRAME_HEADER;
    LobotTxBuf[2] = 2;  // 数据长度，数据帧除帧头部分数据字节数，此命令固定为2
    LobotTxBuf[3] = CMD_GET_BATTERY_VOLTAGE;  // 填充获取电池电压命令

    HAL_UART_Transmit(SE_huart, LobotTxBuf, 4, HAL_MAX_DELAY);  // 发送数据
}

// 接收数据回调函数覆写
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
    if (huart == SE_huart) {  // 判断是否为舵机控制器
        if (LobotRxBuf[0] == FRAME_HEADER && LobotRxBuf[1] == FRAME_HEADER) {
            // 处理接收到的数据
            batteryVolt = (((uint16_t)(LobotRxBuf[5])) << 8) | (LobotRxBuf[4]);
        }
        HAL_UART_Receive_DMA(SE_huart, LobotRxBuf, 16);  // 重新开始接收数据
    }
}